package novemberBot.mapmaking;

import novemberBot.FactoryFloorRobot;
import novemberBot.sensor.UsDataResponder;
import novemberBot.utils.Trig;

/**Map object polls sensor Data and builds a virtual map of environement
 * @author Antoine Lennartz*/
public class Map extends UsDataResponder implements Runnable{
	/**2D byte array containing sensor Data*/
	private byte[][] sensorData;
	/**Lock object for synchronizing sensorData*/
	private Object dataLock = new Object();
	/**if true the map does not read data from the sensors*/
	private boolean paused=false;
	/**Threshold for deciding there is an object in the square*/
	public static final byte OCCUPIED=10;
	/**Constructor
	 * @param s Sensor that is needd for this map (high or low)
	 * @param size size of this map*/
	public Map(Sensor s, int size) {
		super("median2", s);
		sensorData = new byte[size][size];
		filter.setFilterAmount(7);
		filter.newMeasurement();
		filter.setMaxD(60);
	}
	/**accessor for size
	 * @return size of sensor Data array*/
	public int getSize(){
		return sensorData.length;
	}
	
	/**marks the map to indicate that something is present
	 * @param x - x coord to mark
	 * @param y - y coord to mark*/
	private void darken(int x, int y){
		if(getSensorData()[x][y]<127)getSensorData()[x][y]++;
	}
	/**marks the map to indicate that something is absent
	 * @param x - x coord to mark
	 * @param y - y coord to mark*/
	private void lighten(int x, int y){
		if(getSensorData()[x][y]>-1)getSensorData()[x][y]--;
	}
	
	
	/**static method for summing map sensor data
	 * @param a - Map a 
	 * @param b - Map b
	 * @return 2D byte array representing the sum of a and b's sensorData array's*/
	public static byte[][] sum(Map a, Map b){
		byte[][] sum = new byte[a.getSensorData().length][a.getSensorData()[0].length];
		int s;
		for(int i=0; i<a.getSensorData().length; i++){
			for(int j=0; j<a.getSensorData()[0].length; j++){
				s=a.getSensorData()[i][j]+b.getSensorData()[i][j];
				if(s>127)s=127;
				sum[i][j]=(byte)s;
			}
		}
		return sum;
	}
	

	/**static method for subtracting map sensor data
	 * @param a - Map a 
	 * @param b - Map b
	 * @param bscale - factor to scale b values by before subtracting
	 * @return 2D byte array representing the subtraction: [a-bscale*b] for sensorData*/
	public static byte[][] subtract(Map a, Map b, double bscale){
		byte[][] diff = new byte[a.getSensorData().length][a.getSensorData()[0].length];
		int d;
		for(int i=0; i<a.getSensorData().length; i++){
			for(int j=0; j<a.getSensorData()[0].length; j++){
				d=(int) (a.getSensorData()[i][j]-bscale*b.getSensorData()[i][j]);
				if(d<-1)d=-1;
				diff[i][j]=(byte)d;
			}
		}
		return diff;
	}
	/**converts normal coordinates (mm precision) to map coords (ft precision)
	 * @param odoPos - coordinates in odometry or high resolution form
	 * @return coordinates in map form */
	public static int[] mapCoords(double[] odoPos){
		int[] mapCoords = new int[2];
		mapCoords[0]=(int)Math.round(odoPos[0]/30.48);
		mapCoords[1]=(int)Math.round(odoPos[1]/30.48);
		//if(mapCoords)
		return mapCoords;
	}
	
	/**converts map coordinates (ft precision)to normal coords (mm precision)
	 * @param mapCoords - coordinates in map form
	 * @return coordinates in odometry or high resolution form*/
	public static double[] odoCoords(int[] mapCoords){
		double[] odoCoords = new double[2];
		odoCoords[0]=mapCoords[0]*30.48;
		odoCoords[1]=mapCoords[1]*30.48;
		return odoCoords;
		
	}
	/**checks to see if certain coordinates of current map are occupied or not
	 * @param mapCoords
	 * @return boolean comparing sensorData to Map.OCCUPIED*/
	public boolean occupied(int[] mapCoords){
		return sensorData[mapCoords[0]][mapCoords[1]]>OCCUPIED;
	}
	
	/**synchronized accessor for sensorData
	 * @return sensorData - 2D byte array*/
	public byte[][] getSensorData(){
		synchronized(dataLock){
			return sensorData;
		}
	}
	/**pauses the mapmaking process*/
	public void pause(){
		paused=true;
		needsData=false;
	}
	/**resumes the mapmaking process*/
	public void resume(){
		needsData=true;
		filter.newMeasurement();
		paused=false;
		
	}
	/**returns current position in map coordinates form instead of odometer form*/
	public static int[] currentMapPos(){
		double[] cPos = new double[3];
		FactoryFloorRobot.getInstance().getOdometer().getPosition(cPos);
		return mapCoords(cPos);
	}
	/**Checks sensor Data and lightens or darkens the relevant map areas.
	 * @see Map#lighten(int x, int y)
	 * @see Map#darken(int x, int y)*/
	@Override
	public void run() {
		needsData=true;
		while(true){
			while(paused)Thread.yield();
			int d = filter.getData();
			double[] pos = getSensorPosition();
			int[] mapPos = Map.mapCoords(pos);
			pos[0]+=d*Trig.sin2(pos[2]);
			pos[1]+=d*Trig.cos2(pos[2]);
			int[] targPos=Map.mapCoords(pos);			
			int sX, eX, sY, eY;//lightening boundaries
			if(d!=255){
				darken(targPos[0], targPos[1]);
				if(mapPos[0]<targPos[0]){
					sX=mapPos[0]+1;
					eX=targPos[0]-1;
				}else{
					sX=targPos[0]+1;
					eX=mapPos[0]-1;
				}
				if(mapPos[1]<targPos[1]){
					sY=mapPos[1]+1;
					eY=targPos[1]-1;
				}else{
					sY=targPos[1]+1;
					eY=mapPos[1]-1;
				}	
			}else{//did not see anything
				if(mapPos[0]<targPos[0]){
					sX=mapPos[0]+1;
				}else{
					sX=mapPos[0]-2;
				}
				eX=sX+1;
				if(mapPos[1]<targPos[1]){
					sY=mapPos[1]+1;
				}else{
					sY=mapPos[1]-2;
				}
				eY=sY+1;
			}
			if(sX<0)sX=0;
			if(sX>12)sX=12;
			if(sY<0)sY=0;
			if(sY>12)sY=12;
			if(eX<0)eX=0;
			if(eX>12)eX=12;
			if(eY<0)eY=0;
			if(eY>12)eY=12;
			
			for(int i=sX; i<=eX; i++){
				for(int j=sY; j<=eY; j++){
					lighten(i, j);
				}
			}
		}
		
	}
}
